/*
	Box Sensor Functions
	Author: Jared Bayne (JaredNBayne@gmail.com)
*/

#include <math.h>

#define threshold_setter_right			20//12			//amount to subtract from track reading to create threshold
#define threshold_setter_left			17//9			//amount to subtract from track reading to create threshold
#define threshold_setter_front_left		14//8			//amount to subtract from track reading to create threshold
#define threshold_setter_front_right 	7//3			//amount to subtract from track reading to create threshold

#define	threshold_box		150		//amount + threshold that is threshold for box...>this num means we see box

// PASS: (Pass)anger, Drive: (drive)r-side
#define	edge_front_left		1
#define edge_front_right   	0		
#define edge_back   		999		//don't have this sensor right now
#define edge_pass_front   	2		
#define edge_pass_back   	33		
#define edge_drive_front   	3		
#define edge_drive_back   	99		//dont have this sensor right now


#define parallel 0
#define angled_towards 1
#define angled_away 2
#define both_away 3



// Definitions of edge sensors
// We have no back edge sensor 4/5/09
/*
		 		    top view of bot
			  	  	edge_front	(1)
				    ------------
				   |	front	|
edge_drive_front(0)|			|edge_pass_front (2)
				   |			|
				   |			|
edge_drive_back(99)|			|edge_pass_back (3)
				   |			|
				    ------------
				    edge_back (99)


*/

// the measurement is 160 +- 5  155-165 relably while moving!
// for safety reason, I would suggest (plus or minus) +- of 10

int edge_front_left_threshold	=	132;
int edge_front_right_threshold	=	132;
int edge_drive_back_threshold 	=	175;
int edge_drive_front_threshold 	=	192;
int edge_pass_front_threshold 	=	182;
int edge_pass_back_threshold 	=	175;
int edge_back_threshold			=	175;

//private funcitons
int read_edge(int sensor);
int does_sense_see_edge(int sensor);	//does sensor see edge?

//public functions
void box_detector_edge_see_tester(int sensor);
void box_sensor_tester(void);
void box_sensor_tester_manual(void);
int see_edge_front(void);
int see_edge_back(void);
int parallel_drive(void);
int parallel_pass(void);
void self_calibrate_edge_detectors(void);
int double_check_edge(int sensor, int num_confirm);
int double_check_edge_all(int num_confirm, int num_confirm_front);

int super_confirm_see_floor(int num_readings);
int super_confirm_see_box(int num_readings);
int double_check_box(int sensor, int num_confirm);
int edge_sensor_see_box(void);

// Sensor numbers are define as above in Ascii art
int does_sense_see_edge(int sensor)
{
/*
	returns:
			yes -> sensor sees edge
			no -> sensor doesn't see edge (sees track)


*/

// This is used to find the data that comes out of front left sensor


	switch(sensor)
	{
		case edge_front_left:
			if (read_edge(edge_front_left)<edge_front_left_threshold)
				return YES;
			else	
				return NO;
			break;
		case edge_front_right:
			if (read_edge(edge_front_right)<edge_front_right_threshold)
				return YES;
			else	
				return NO;
			break;
		case edge_drive_front:
			if ( read_edge(edge_drive_front) < edge_drive_front_threshold) {
				return YES;
			} else
				return NO;
			break;
			
		case edge_drive_back:
			if (read_edge(edge_drive_back)<edge_drive_back_threshold)
				return YES;
			else	
				return NO;
			break;
			
		case edge_pass_front:
			if (read_edge(edge_pass_front)<edge_pass_front_threshold)
				return YES;
			else	
				return NO;
			break;
			
		case edge_pass_back:
			if (read_edge(edge_pass_back)<edge_pass_back_threshold)
				return YES;
			else	
				return NO;
			break;
		
		case edge_back:
			if (read_edge(edge_back)<edge_back_threshold)
				return YES;
			else	
				return NO;
			break;
			
		default:	//back sensor value
			writeln("Bad sensor");
			usart_writeln("Bad Sensor",1);
			return no;	
			break;
	
	
	}

}

int does_sense_see_box(int sensor)
{
/*
	returns:
			yes -> sensor sees box
			no -> sensor doesn't see box


*/

// This is used to find the data that comes out of front left sensor


	switch(sensor)
	{
		case edge_front_left:
			if (read_edge(edge_front_left)>edge_front_left_threshold+threshold_box)
				return YES;
			else	
				return NO;
			break;
		case edge_front_right:
			if (read_edge(edge_front_right)>edge_front_right_threshold+threshold_box)
				return YES;
			else	
				return NO;
			break;
		case edge_drive_front:
			if ( read_edge(edge_drive_front) > edge_drive_front_threshold+threshold_box) {
				return YES;
			} else
				return NO;
			break;
			
		case edge_drive_back:
			if (read_edge(edge_drive_back)>edge_drive_back_threshold+threshold_box)
				return YES;
			else	
				return NO;
			break;
			
		case edge_pass_front:
			if (read_edge(edge_pass_front)>edge_pass_front_threshold+threshold_box)
				return YES;
			else	
				return NO;
			break;
			
		case edge_pass_back:
			if (read_edge(edge_pass_back)>edge_pass_back_threshold+threshold_box)
				return YES;
			else	
				return NO;
			break;
		
		case edge_back:
			if (read_edge(edge_back)>edge_back_threshold+threshold_box)
				return YES;
			else	
				return NO;
			break;
			
		default:	//back sensor value
			writeln("Bad sensor");
			usart_writeln("Bad Sensor",1);
			return no;	
			break;
	
	
	}

}

int parallel_drive(void)
{
/*
	returns:
			parallel -> lined up parallel
			angled_towards -> front sees edge back doesn't
			angled_away -> back sees edge front doesn't
			both_away -> both don't see edge
			
	NOTE:
		Not using this function at this time 
		because we don't have two sensors per side
		as needed by this function.

*/

	switch(does_sense_see_edge(edge_drive_front))
	{
		case yes:
			switch(does_sense_see_edge(edge_drive_back))
			{	
				case  yes:
					//both sensors see edge
					return parallel;
					break;
				case no:
					//front sees edge back doesn't
					return angled_towards;
					break;
				default:
					writeln("Bad edge value");
					usart_writeln("Bad edge value",1);
					break;
			
			}
			break;
		case no:
			switch(does_sense_see_edge(edge_drive_back))
			{	
				case  yes:
					//front doesn't see back does
					return angled_away;
					break;
				case no:
					//both don't see edge
					return both_away;
					break;
				default:
					writeln("Bad edge value");
					usart_writeln("Bad edge value",1);
					break;
			
			}
			break;
		default:
			writeln("Bad return");
			usart_writeln("Badd return from does_sense_see_edge()",1);
	
	
	}

	return both_away;	//should never reach here


}

int parallel_pass(void)
{
/*
	returns:
			parallel -> lined up parallel
			angled_towards -> front sees edge back doesn't
			angled_away -> back sees edge front doesn't
			both_away -> both don't see edge
	NOTE:
		Not using this function at this time 
		because we don't have two sensors per side
		as needed by this function.
*/

	switch(does_sense_see_edge(edge_drive_front))
	{
		case yes:
			switch(does_sense_see_edge(edge_drive_back))
			{	
				case  yes:
					//both sensors see edge
					return parallel;
					break;
				case no:
					//front sees edge back doesn't
					return angled_towards;
					break;
				default:
					writeln("Bad edge value");
					usart_writeln("Bad edge value",1);
					break;
			
			}
			break;
		case no:
			switch(does_sense_see_edge(edge_drive_back))
			{	
				case  yes:
					//front doesn't see back does
					return angled_away;
					break;
				case no:
					//both don't see edge
					return both_away;
					break;
				default:
					writeln("Bad edge value");
					usart_writeln("Bad edge value",1);
					break;
			
			}
			break;
		default:
			writeln("Bad return");
			usart_writeln("Badd return from does_sense_see_edge()",1);
	
	}
	
	return both_away;	//should never reach here

}

void box_sensor_tester(void)
{	

/*	Simply a tester function for the edge sensors.
	
	Modify as necessary to output data corresponding
	to sensors you are working on.
*/
	writeln("see usart out");
	
	usart_writeln("frontL: ",1);
	usart_disp_int(read_edge(edge_front_left),1);
	//usart_write("     edge_back: ",1);
	//usart_disp_int(read_edge(edge_back),1);
	//usart_write("     drive_front: ",1);
	//usart_disp_int(read_edge(edge_drive_front),1);
	usart_write("     pass_front: ",1);
	usart_disp_int(read_edge(edge_pass_front),1);
	usart_write("     pass_back: ",1);
	usart_disp_int(read_edge(edge_pass_back),1);
	//usart_write("     edge_drive_back: ",1);
	//usart_disp_int(read_edge(edge_drive_back),1);	
	
	_delay_ms(500);
}
void box_sensor_tester_manual(void)
{	
/*
	Another edge sensor tester, but
	it will wait for the user to press
	a button before it displays new output.
*/
	
	writeln("see usart out");
	
	usart_writeln("frontL: ",1);
	usart_disp_int(read_edge(edge_front_left),1);
	//usart_write("     edge_back: ",1);
	//usart_disp_int(read_edge(edge_back),1);
	usart_write("     drive_front: ",1);
	usart_disp_int(read_edge(edge_drive_front),1);
	usart_write("     pass_front: ",1);
	usart_disp_int(read_edge(edge_pass_front),1);
	usart_write("     pass_back: ",1);
	usart_disp_int(read_edge(edge_pass_back),1);
	//usart_write("     edge_drive_back: ",1);
	//usart_disp_int(read_edge(edge_drive_back),1);	
	
	wait_for_button_press();
}

int read_edge(int sensor)
{
/*
	Read the Analog to Digital Converter corresponding
	to the sensor passed in by parameter.
	
	This is a very low-level routing.  Please don't mess with it
	unless you know what you are doing.

*/


	//Take the reading
	 int temp=0; // This is the name of the sensor which is being processed (We go through 8 of them)
	 int i=0;
	 int ADCresult=0;
	 
	 
	 //take 1 reading and throw it out
	 ADCSRA = _BV(ADEN)|_BV(ADPS1)|_BV(ADPS0);
	 ADMUX = (_BV(REFS0) | _BV(REFS1)) +sensor;
	  for(i=0;i < 8;i++)		//Throw away the first 8 readings
	 {
	  ADCSRA |= _BV(ADSC);
	  while(ADCSRA & _BV(ADSC));
	 }
	 
	 for(i=0;i < 16;i++)
	 {
	  ADCSRA |= _BV(ADSC);
	  while(ADCSRA & _BV(ADSC));
	  temp = ADCL;
	  temp += (ADCH << 8);
	  i++; 
	  ADCresult += temp;
	
	 
	 }
	 
	 // Damn it, the debuge command here just sends garbage. If you take the (char*) out, then it will just clear the reciever's screen - Dustin McBeth 4/4/09
	 if(DEBUG_EDGE_SENSOR) { usart_disp_int(temp,1);} //  usart_write((char*)ADCresult,1); }// this should print out distance of sensor from ground, larger or smaller is further? - Dustin McBeth 4/4/09
	return (ADCresult >> 4);// divide by 8 for average



}

void box_detector_edge_see_tester(int sensor)
{
/*
	This function will display a reading of the sensor
	on the LCD then display whether it corresponds
	to an edge value or track value
	
	NOTE:  the value actually displayed isn't the exact
	value sent to the logic function (determines whether
	the value corresponds to edge or track).



*/
		writeln(": ");
		lcddispint(read_edge(sensor));
		write("   ");
		lcddispint(does_sense_see_edge(sensor));
		_delay_ms(500);


}

void self_calibrate_edge_detectors(void)
{
/*	
	DESCRIPTION:
		This function calibrates the edge sensors prior to every run (if it is called 
		before every run as it should be).
		
		Why do we need this?  Well, what if someone knocks the edge sensors just a little
		bit and the sensors are now just a hair closer or further from the track.  This will
		change the readings and possible cause catastrophic failure and Robin to cry because
		the robot just nose dived off the board.
		
		We also need it in case the sensors just happen to decide to read out different values.
		They can do this if the reference voltage changes.  The reference voltage shouldn't ever
		change, but just in case it is best to self calibrate like this.
		
	FUNCTIONALITY:
		Takes thousands of readings of each sensor and averages them out.
		
		Currently I have it set to num_readings=5000.
		
		This means it will call the sensor read function 5,000 per sensor.  If you go
		look at the actual read_edge() function you will see that we take numerous readings
		in there as well and average them out.
		
		So, with a value of num_readings=5,000 we are effectively averaging out 80,000 readings
		per sensor.  So a total of 240,000 readings are taken (3 sensors to average).  Quite a 
		bit of readings in just the short 1-2 seconds it takes to run this function.


*/
	double edge_front_left_sum=0, edge_front_right_sum=0, edge_drive_front_sum=0, edge_pass_front_sum=0;
	int num_readings = 10000;

	writeln("Cal: Put in CTR");
	lcdline2();
	write("Press btn ready");	
	wait_for_button_press();

	writeln("Calibrating...");
	for (int i=0;i<num_readings;i++)
	{
		edge_drive_front_sum	+=	read_edge(edge_drive_front);
		edge_pass_front_sum		+=	read_edge(edge_pass_front);
		edge_front_left_sum		+=	read_edge(edge_front_left);
		edge_front_right_sum	+=	read_edge(edge_front_right);
	
	}
	
	edge_drive_front_threshold = floor((edge_drive_front_sum/num_readings)-threshold_setter_left);
	edge_pass_front_threshold = floor((edge_pass_front_sum/num_readings)-threshold_setter_right);
	edge_front_left_threshold = floor((edge_front_left_sum/num_readings)-threshold_setter_front_left);
	edge_front_right_threshold = floor((edge_front_right_sum/num_readings)-threshold_setter_front_right);


	writeln(" L:"); lcddispint(edge_drive_front_threshold);
	write(" FL:"); lcddispint(edge_front_left_threshold);	
	lcdline2();
	write("FR:"); lcddispint(edge_front_right_threshold);
	write(" R:"); lcddispint(edge_pass_front_threshold);
	wait_for_button_press();
	//_delay_ms(1000);
	
}

int double_check_edge(int sensor, int num_confirm)
{
	/*
		RETURNS: 	YES - sensor sees edge
					NO	- sensor sees track
	
		
		Our sensors can give us random readings once in a while.  If we took every
		reading as true then we will have problems.  For instance, a good reading
		is between 150-250.  Once in a while we'll get a reading of 25.  Obviously
		this has to be disregarded.  The way we handle this is to look for a certain
		number of good readings in a row.  This "number" is defined as "num_confirm"
		in this function's parameters.
	
		Errs on the side of stopping early.
	
	*/

	int checker=0;
	if (does_sense_see_edge(sensor))	//got one reading that says edge
	{
		checker=0;
		for (int i=0;i<num_confirm-1;i++)	//see if we can get CONFRIM_EDGE readings in a row
		{
			if (!does_sense_see_edge(sensor))
				checker = 1;
		
		
		}
		
		if (checker==0)		//we saw edge CONFIRM_EDGE times
		{
			//turn to the right a little bit
			writeln("Saw Edge: ");
			lcddispint(sensor);
			return YES;
	
		}
	
	
	}
	else
		return NO;
		
	return NO;
	
}

int double_check_track(int sensor, int num_confirm)
{
	/*
		RETURNS: 	YES - sensor sees track
					NO - sensor sees edge
	
		
		Our sensors can give us random readings once in a while.  If we took every
		reading as true then we will have problems.  For instance, a good reading
		is between 150-250.  Once in a while we'll get a reading of 25.  Obviously
		this has to be disregarded.  The way we handle this is to look for a certain
		number of good readings in a row.  This "number" is defined as "num_confirm"
		in this function's parameters.
	
		Errs on the side of stopping early.
	*/

	int checker=0;
	if (!does_sense_see_edge(sensor))	//got one reading that says edge
	{
		checker=0;
		for (int i=0;i<num_confirm-1;i++)	//see if we can get CONFRIM_EDGE readings in a row
		{
			if (does_sense_see_edge(sensor))
				checker = 1;
		
		
		}
		
		if (checker==0)		//we saw edge CONFIRM_EDGE times
		{
			//turn to the right a little bit
		
			return YES;
	
		}
	
	
	}
	else
		return NO;
		
	return NO;
	
}
int double_check_edge_all(int num_confirm_reg, int num_confirm_front)
{
/*		RETURNS: 	YES - all sensors see track
					NO - at least 1 sensor sees edge
	
		
		Our sensors can give us random readings once in a while.  If we took every
		reading as true then we will have problems.  For instance, a good reading
		is between 150-250.  Once in a while we'll get a reading of 25.  Obviously
		this has to be disregarded.  The way we handle this is to look for a certain
		number of good readings in a row.  This "number" is defined as "num_confirm"
		in this function's parameters.
	
*/
	if (!double_check_edge(edge_front_left, num_confirm_front) && !double_check_edge(edge_front_right, num_confirm_front) && !double_check_edge(edge_pass_front, num_confirm_reg) && !double_check_edge(edge_drive_front, num_confirm_reg))
		return YES;	//all sensors see track
	return NO;	//at least 1 sensor sees edge
	
}

int super_confirm_see_floor(int num_readings)
{
	double edge_front_left_sum=0, edge_front_right_sum=0, edge_drive_front_sum=0, edge_pass_front_sum=0;
	
	for (int i=0;i<num_readings;i++)
	{
		edge_drive_front_sum	+=	read_edge(edge_drive_front);
		edge_pass_front_sum		+=	read_edge(edge_pass_front);
		edge_front_left_sum		+=	read_edge(edge_front_left);
		edge_front_right_sum	+=	read_edge(edge_front_right);
	
	}

		if ((edge_drive_front_sum/num_readings)<edge_drive_front_threshold)
			return YES;
			
		if ((edge_pass_front_sum/num_readings)<edge_pass_front_threshold)
			return YES;
		
		if ((edge_front_left_sum/num_readings)<edge_front_left_threshold)
			return YES;
		
		if ((edge_front_right_sum/num_readings)<edge_front_right_threshold)
			return YES;
	
		return NO;
		

}

int super_confirm_see_box(int num_readings)
{
	/*
		RETURNS:
			YES -> see box
			NO	-> no box seen
	
	
	*/
	
	
	double edge_front_left_sum=0, edge_front_right_sum=0, edge_drive_front_sum=0, edge_pass_front_sum=0;

	for (int i=0;i<num_readings;i++)
	{
		edge_drive_front_sum	+=	read_edge(edge_drive_front);
		edge_pass_front_sum		+=	read_edge(edge_pass_front);
		edge_front_left_sum		+=	read_edge(edge_front_left);
		edge_front_right_sum	+=	read_edge(edge_front_right);
	
	}

	
		if ((edge_front_left_sum/num_readings)<edge_front_left_threshold+50)
			return YES;
		
		if ((edge_front_right_sum/num_readings)<edge_front_right_threshold+50)
			return YES;
	
		return NO;
		

}

int double_check_box(int sensor, int num_confirm)
{
	/*
		RETURNS: 	YES - sensor sees box
					NO - sensor sees no box
	
		
	*/

	int checker=0;
	if (does_sense_see_box(sensor))	//got one reading that says box
	{
		checker=0;
		for (int i=0;i<num_confirm-1;i++)	//see if we can get CONFRIM_EDGE readings in a row
		{
			if (!does_sense_see_box(sensor))
				checker = 1;
		
		
		}
		
		if (checker==0)		//we saw box CONFIRM_EDGE times
		{
		
			return YES;
	
		}
	
	
	}
	else
		return NO;
		
	return NO;
	
}


int edge_sensor_see_box(void)
{
/*
	checks if we see the box or not for sure
*/
	int flag = 0;

	if (read_edge(edge_front_left)>(edge_front_left_threshold+threshold_box))
	{		
		flag=0;
		for (int i = 0; i<10; i++)
		{
			if (read_edge(edge_front_left)<(edge_front_left_threshold+threshold_box))
				flag++;		
		}
		
		if (flag==0)	//saw 10 in a row
			return YES;
	}
	if (read_edge(edge_front_right)>(edge_front_right_threshold+threshold_box))
	{		
		flag=0;
		for (int i = 0; i<10; i++)
		{
			if (read_edge(edge_front_right)<(edge_front_right_threshold+threshold_box))
				flag++;			
		}
		
		if (flag==0)	//saw 10 in a row
			return YES;
		
	}
	if (read_edge(edge_drive_front)>(edge_drive_front_threshold+threshold_box))
	{		
		flag=0;
		for (int i = 0; i<10; i++)
		{
			if (read_edge(edge_drive_front)<(edge_drive_front_threshold+threshold_box))
				flag++;			
		}
		
		if (flag==0)	//saw 10 in a row
			return YES;
		
	}
	if (read_edge(edge_pass_front)>(edge_pass_front_threshold+threshold_box))
	{		
		flag=0;
		for (int i = 0; i<10; i++)
		{
			if (read_edge(edge_pass_front)<(edge_pass_front_threshold+threshold_box))
				flag++;			
		}
		
		if (flag==0)	//saw 10 in a row
			return YES;
		
	}
	return NO;
}
